﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Body.h
// Revision   : r141
// Change-Date: 2010-10-10
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Body.cpp
// Revision   : r143
// Change-Date: 2011-02-03
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>The body type. <br/>
    /// static: zero mass, zero velocity, may be manually moved <br/>
    /// kinematic: zero mass, non-zero velocity set by user, moved by solver <br/>
    /// dynamic: positive mass, non-zero velocity determined by forces, moved by solver <br/>
    /// </summary>
    public enum BodyType
    {
        /// <summary>
        /// zero mass, zero velocity, may be manually moved
        /// </summary>
        STATIC = 0,
        /// <summary>
        /// zero mass, non-zero velocity set by user, moved by solver
        /// </summary>
        KINEMATIC,
        /// <summary>
        /// positive mass, non-zero velocity determined by forces, moved by solver
        /// </summary>
        DYNAMIC,
    }; 
 
    /// <summary>A body definition holds all the data needed to construct a rigid body.
    /// You can safely re-use body definitions.</summary>
    public class BodyDef
    {
        /// This constructor sets the body definition default values.
        public BodyDef()
        {
            userData = null;
            position = Vector2.Zero;
            angle = 0.0f;
            linearVelocity = Vector2.Zero;
            angularVelocity = 0.0f;
            linearDamping = 0.0f;
            angularDamping = 0.0f;
            allowSleep = true;
            awake = true;
            fixedRotation = false;
            bullet = false;
            type = BodyType.STATIC;
            active = true;
            inertiaScale = 1.0f;
        }

        /// <summary>The body type: static, kinematic, or dynamic.</summary>
        /// <remarks>if a dynamic body would have zero mass, the mass is set to one.</remarks>
        public BodyType type;

        /// <summary>The world position of the body. Avoid creating bodies at the origin
        /// since this can lead to many overlapping shapes.</summary>
        public Vector2 position;

        /// <summary>The world angle of the body in radians.</summary>
        public float angle;

        /// <summary>The linear velocity of the body's origin in world co-ordinates.</summary>
        public Vector2 linearVelocity;

        /// <summary>The angular velocity of the body.</summary>
        public float angularVelocity;

        /// <summary>Linear damping is use to reduce the linear velocity. The damping parameter
        /// can be larger than 1.0f but the damping effect becomes sensitive to the
        /// time step when the damping parameter is large.</summary>
        public float linearDamping;

        /// <summary>Angular damping is use to reduce the angular velocity. The damping parameter
        /// can be larger than 1.0f but the damping effect becomes sensitive to the
        /// time step when the damping parameter is large.</summary>
        public float angularDamping;

        /// <summary>Set this flag to false if this body should never fall asleep. Note that
        /// this increases CPU usage.</summary>
        public bool allowSleep;

        /// <summary>Is this body initially awake or sleeping?</summary>
        public bool awake;

        /// <summary>Should this body be prevented from rotating? Useful for characters.</summary>
        public bool fixedRotation;

        /// <summary>Is this a fast moving body that should be prevented from tunneling through
        /// other moving bodies? Note that all bodies are prevented from tunneling through
        /// kinematic and static bodies. This setting is only considered on dynamic
        /// bodies.
        /// </summary>
        /// <remarks>You should use this flag sparingly since it increases processing time.</remarks>
        public bool bullet;

        /// <summary>Does this body start out active?</summary>
        public bool active;

        /// <summary>Use this to store application specific body data.</summary>
        public Object userData;

        /// <summary>Experimental: scales the inertia tensor.</summary>
        public float inertiaScale;
    };

    /// <summary>A rigid body. These are created via World.CreateBody.</summary>
    public class Body 
    {
        /// <summary>Creates a fixture and attach it to this body.</summary>
	    /// <param name="def">the fixture definition.</param>
	    /// <remarks>This function is locked during callbacks.</remarks>
        public Fixture CreateFixture(FixtureDef def)
        {
            System.Diagnostics.Debug.Assert(m_world.IsLocked() == false);
            if (m_world.IsLocked() == true) return null;

            Fixture fixture = new Fixture();
            fixture.Create(this, def);

            if ((m_flags & BodyFlags.e_activeFlag) == BodyFlags.e_activeFlag)
            {
                BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
                fixture.CreateProxies(broadPhase, m_xf);
            }

            fixture.m_next = m_fixtureList;
            m_fixtureList = fixture;
            ++m_fixtureCount;

            fixture.m_body = this;

            // Adjust mass properties if needed.
            if (fixture.m_density > 0.0f)
            {
                ResetMassData();
            }

            // Let the world know we have a new fixture. This will cause new contacts         
            // to be created at the beginning of the next time step.         
            m_world.m_flags |= World.WorldFlags.e_newFixture;          
            
            return fixture; 
        }

        public Fixture CreateFixture(Shape shape, float density)
        {
            FixtureDef def = new FixtureDef();
            def.shape = shape;
            def.density = density;

            return CreateFixture(def);
        }

        /// <summary>Destroy a fixture. This removes the fixture from the broad-phase and
	    /// therefore destroys any contacts associated with this fixture. All fixtures
	    /// attached to a body are implicitly destroyed when the body is destroyed.</summary>
	    /// <param name="fixture">the fixture to be removed.</param>
	    /// <remarks>This function is locked during callbacks.</remarks>
        public void DestroyFixture(Fixture fixture)
        {
            System.Diagnostics.Debug.Assert(m_world.IsLocked() == false); 
            if (m_world.IsLocked() == true) 
            { 
                return; 
            } 

            System.Diagnostics.Debug.Assert(fixture.m_body == this); 

            // Remove the fixture from this body's singly linked list. 

            // already implemented preliminary fix for box2d issue 123 (http://code.google.com/p/box2d/issues/detail?id=123)
            System.Diagnostics.Debug.Assert(m_fixtureCount > 0); 
            Fixture node = m_fixtureList; 
            bool found = false;
            Fixture prev = m_fixtureList;
            while (node != null) 
            { 
                if (node == fixture) 
                {
                    if (node != m_fixtureList)
                    {
                        prev.m_next = fixture.m_next;
                    }
                    else
                    {
                        m_fixtureList = fixture.m_next;
                    }
                    found = true; 
                    break; 
                }

                prev = node;
                node = node.m_next; 
            } 

            // You tried to remove a shape that is not attached to this body. 
            System.Diagnostics.Debug.Assert(found); 

            // Destroy any contacts associated with the fixture. 
            ContactEdge edge = m_contactList; 
            while (edge != null)
            { 
                Contact c = edge.contact; 
                edge = edge.next; 

                Fixture fixtureA = c.GetFixtureA(); 
                Fixture fixtureB = c.GetFixtureB(); 

                if (fixture == fixtureA || fixture == fixtureB) 
                { 
                     // This destroys the contact and removes it from 
                     // this body's contact list. 
                     m_world.m_contactManager.Destroy(c); 
                } 
            } 

            if ((m_flags & BodyFlags.e_activeFlag) == BodyFlags.e_activeFlag)
            { 
                BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
                fixture.DestroyProxies(broadPhase);
            } 

            fixture.Destroy();
            fixture.m_body = null; 
            fixture.m_next = null; 

            --m_fixtureCount; 

            // Reset the mass data. 
            ResetMassData();  
        }

        /// <summary>Set the position of the body's origin and rotation (radians).
        /// This breaks any contacts and wakes the other bodies.
        /// Manipulating a body's transform may cause non-physical behavior.
        /// </summary>
        /// <param name="position">the new world position of the body's origin (not necessarily the center of mass).</param>
        /// <param name="angle">the new world rotation angle of the body in radians.</param>
        public void SetTransform(Vector2 position, float angle)
        {
            System.Diagnostics.Debug.Assert(m_world.IsLocked() == false);
            if (m_world.IsLocked() == true)
            {
                return;
            }

            m_xf.R.Set(angle);
            m_xf.position = position;

#if !OPTIMIZE
            m_sweep.c0 = m_sweep.c = Math.Mul(m_xf, m_sweep.localCenter);
            m_sweep.a0 = m_sweep.a = angle;
#else
            Vector2 t; t.X = m_xf.R.col1.X * m_sweep.localCenter.X + m_xf.R.col2.X * m_sweep.localCenter.Y; t.Y = m_xf.R.col1.Y * m_sweep.localCenter.X + m_xf.R.col2.Y * m_sweep.localCenter.Y;
            m_sweep.c.X = m_xf.position.X + t.X;
            m_sweep.c.Y = m_xf.position.Y + t.Y;
            m_sweep.c0 = m_sweep.c;
            m_sweep.a0 = m_sweep.a = angle;
#endif

            BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
            for (Fixture f = m_fixtureList; f != null; f = f.m_next)
            {
                f.Synchronize(broadPhase, ref m_xf, ref m_xf);
            }

            m_world.m_contactManager.FindNewContacts();             
        }

        /// <summary>Get the body transform for the body's origin.</summary>
        /// <returns>the world transform of the body's origin.</returns>
        public Transform Transform
        {
            get { return m_xf; }
        }

        /// <summary>Get the world body origin position.</summary>
        /// <returns>the world position of the body's origin.</returns>
        public Vector2 Position
        {
            get { return m_xf.position; }
        }

        /// <summary>Get the angle in radians.</summary>
        /// <returns>the current world rotation angle in radians.</returns>
        public float Angle
        {
            get { return m_sweep.a; }
        }

        /// <summary>Get the world position of the center of mass.</summary>
        public Vector2 WorldCenter
        {
            get { return m_sweep.c; }
        }

        /// <summary>Get the local position of the center of mass.</summary>
        public Vector2 LocalCenter
        {
            get { return m_sweep.localCenter; }
        }

        /// <summary>Get the linear velocity of the center of mass.</summary>
        /// <returns>the linear velocity of the center of mass.</returns>
        public Vector2 LinearVelocity
        {
            get { return m_linearVelocity; }
            set
            {
                if (m_type == BodyType.STATIC) return;

                if (Vector2.Dot(value, value) > 0.0f) Awake = true;

                m_linearVelocity = value;
            }
        }

        /// <summary>Get the angular velocity.</summary>
        /// <returns>the angular velocity in radians/second.</returns>
        public float AngularVelocity
        {
            get { return m_angularVelocity; }
            set
            {
                if (m_type == BodyType.STATIC) return;

                if (value * value > 0.0f) Awake = true;

                m_angularVelocity = value;
            }
        }

        /// <summary>Apply a force at a world point. If the force is not
        /// applied at the center of mass, it will generate a torque and
        /// affect the angular velocity. This wakes up the body.</summary>
        /// <param name="force">the world force vector, usually in Newtons (N).</param>
        /// <param name="point">the world position of the point of application.</param>
        public void ApplyForce(Vector2 force, Vector2 point)
        {
            if (m_type != BodyType.DYNAMIC) return;
            if (Awake == false) Awake = true;

            m_force += force;
            m_torque += Vector2Extensions.Cross(point - m_sweep.c, force);
        }

        /// <summary>Apply a torque. This affects the angular velocity
        /// without affecting the linear velocity of the center of mass.
        /// This wakes up the body.</summary>
        /// <param name="torque">about the z-axis (out of the screen), usually in N-m.</param>
        public void ApplyTorque(float torque)
        {
            if (m_type != BodyType.DYNAMIC) return;
            if (Awake == false) Awake = true;

            m_torque += torque;
        }

        /// <summary>Apply an impulse at a point. This immediately modifies the velocity.
        /// It also modifies the angular velocity if the point of application
        /// is not at the center of mass. This wakes up the body.</summary>
        /// <param name="impulse">the world impulse vector, usually in N-seconds or kg-m/s.</param>
        /// <param name="point">the world position of the point of application.</param>
        public void ApplyLinearImpulse(Vector2 impulse, Vector2 point)
        {
            if (m_type != BodyType.DYNAMIC) return;
            if (Awake == false) Awake = true;

            m_linearVelocity += m_invMass * impulse;
            m_angularVelocity += m_invI * Vector2Extensions.Cross(point - m_sweep.c, impulse);
        }

        /// <summary>
        /// Apply an angular impulse.
        /// </summary>
        /// <param name="impulse">the angular impulse in units of kg*m*m/s</param>
        public void ApplyAngularImpulse(float impulse)
        {
            if (m_type != BodyType.DYNAMIC) return;
            if (Awake == false) Awake = true;

            m_angularVelocity += m_invI * impulse;
        }

        /// <summary>Get the total mass of the body.</summary>
        /// <returns>the mass, usually in kilograms (kg).</returns>
        public float Mass
        {
            get { return m_mass; }
        }

        /// <summary>Get the rotational inertia of the body about the local origin.</summary>
        /// <returns>the rotational inertia, usually in kg-m^2.</returns>
        public float Inertia
        {
            get { return m_I + m_mass * Vector2.Dot(m_sweep.localCenter, m_sweep.localCenter); }
        }

        /// <summary>Get the mass data of the body.</summary>
        /// <param name="data">out parameter: containing the mass, inertia and center of the body.</param>
        public void GetMassData(out MassData data)
        {
            data = new MassData();
            data.mass = m_mass;
            data.I = m_I + m_mass * Vector2.Dot(m_sweep.localCenter, m_sweep.localCenter);
            data.center = m_sweep.localCenter;
        }

        /// <summary>Set the mass properties to override the mass properties of the fixtures.<br/>
        /// Note that this changes the center of mass position.<br/>
        /// Note that creating or destroying fixtures can also alter the mass.<br/>
        /// </summary>
        /// <param name="massData">the mass properties.</param>
        /// <remarks>This function has no effect if the body isn't dynamic.</remarks>
        public void SetMassData(MassData massData)
        {
            System.Diagnostics.Debug.Assert(m_world.IsLocked() == false); 
            if (m_world.IsLocked() == true) 
            { 
                 return; 
            } 

            if (m_type != BodyType.DYNAMIC) 
            { 
                 return; 
            } 

            m_invMass = 0.0f; 
            m_I = 0.0f; 
            m_invI = 0.0f; 

            m_mass = massData.mass; 
            if (m_mass <= 0.0f) 
            { 
                 m_mass = 1.0f; 
            } 

            m_invMass = 1.0f / m_mass; 

            if (massData.I > 0.0f && (m_flags & BodyFlags.e_fixedRotationFlag) == 0) 
            { 
                 m_I = massData.I - m_mass * Vector2.Dot(massData.center, massData.center);
                 System.Diagnostics.Debug.Assert(m_I > 0.0f);
                 m_invI = 1.0f / m_I; 
            } 

            // Move center of mass. 
            Vector2 oldCenter = m_sweep.c; 
            m_sweep.localCenter = massData.center; 

#if !OPTIMIZE            
            m_sweep.c0 = m_sweep.c = Math.Mul(m_xf, m_sweep.localCenter);
#else
            Vector2 t; t.X = m_xf.R.col1.X * m_sweep.localCenter.X + m_xf.R.col2.X * m_sweep.localCenter.Y; t.Y = m_xf.R.col1.Y * m_sweep.localCenter.X + m_xf.R.col2.Y * m_sweep.localCenter.Y;
            m_sweep.c.X = m_xf.position.X + t.X;
            m_sweep.c.Y = m_xf.position.Y + t.Y;
            m_sweep.c0 = m_sweep.c;
#endif

            // Update center of mass velocity. 
            m_linearVelocity += Vector2Extensions.Cross(m_angularVelocity, m_sweep.c - oldCenter); 
        }

        public void ResetMassData()
        {
            // Compute mass data from shapes. Each shape has its own density.         
            m_mass = 0.0f;
            m_invMass = 0.0f;
            m_I = 0.0f;
            m_invI = 0.0f;
            m_sweep.localCenter = Vector2.Zero;
            
            // Static and kinematic bodies have zero mass.
            if (m_type == BodyType.STATIC || m_type == BodyType.KINEMATIC)
            {
                m_sweep.c0 = m_sweep.c = m_xf.position;
                return;
            }

            System.Diagnostics.Debug.Assert(m_type == BodyType.DYNAMIC);
            
            // Accumulate mass over all fixtures.
            Vector2 center = Vector2.Zero;
            for (Fixture f = m_fixtureList; f != null; f = f.m_next)
            {
                if (f.m_density == 0.0f)
                {
                    continue;
                }
                
                MassData massData = new MassData();
                f.GetMassData(out massData);
                m_mass += massData.mass;
                center += massData.mass * massData.center;
                m_I += massData.I;
            }
            
            // Compute center of mass.
            if (m_mass > 0.0f)
            {
                m_invMass = 1.0f / m_mass;
                center *= m_invMass;
            }
            else
            {
                // Force all dynamic bodies to have a positive mass.
                m_mass = 1.0f;
                m_invMass = 1.0f;
            }
            
            if (m_I > 0.0f && (m_flags & BodyFlags.e_fixedRotationFlag) == 0)
            {
                // Center the inertia about the center of mass.
                m_I -= m_mass * Vector2.Dot(center, center);
                System.Diagnostics.Debug.Assert(m_I > 0.0f);
                m_invI = 1.0f / m_I;

            }
            else
            {
                m_I = 0.0f;
                m_invI = 0.0f;
            }
            
            // Move center of mass.
            Vector2 oldCenter = m_sweep.c;
            m_sweep.localCenter = center;
#if !OPTIMIZE
            m_sweep.c0 = m_sweep.c = Math.Mul(m_xf, m_sweep.localCenter);
#else
            Vector2 t; t.X = m_xf.R.col1.X * m_sweep.localCenter.X + m_xf.R.col2.X * m_sweep.localCenter.Y; t.Y = m_xf.R.col1.Y * m_sweep.localCenter.X + m_xf.R.col2.Y * m_sweep.localCenter.Y;
            m_sweep.c.X = m_xf.position.X + t.X;
            m_sweep.c.Y = m_xf.position.Y + t.Y;
            m_sweep.c0 = m_sweep.c;
#endif

            // Update center of mass velocity.
            m_linearVelocity += Vector2Extensions.Cross(m_angularVelocity, m_sweep.c - oldCenter); 
        }

        /// <summary>Get the world coordinates of a point given the local coordinates.</summary>
        /// <param name="localPoint">a point on the body measured relative the the body's origin.</param>
        /// <returns>the same point expressed in world coordinates.</returns>
        public Vector2 GetWorldPoint(ref Vector2 localPoint)
        {
            //return Math.Mul(ref m_xf, ref localPoint);
            Vector2 u = new Vector2();
            u.X = m_xf.R.col1.X * localPoint.X + m_xf.R.col2.X * localPoint.Y;
            u.Y = m_xf.R.col1.Y * localPoint.X + m_xf.R.col2.Y * localPoint.Y;
            return m_xf.position + u;
        }

        /// <summary>Get the world coordinates of a point given the local coordinates.</summary>
        /// <param name="localPoint">a point on the body measured relative the the body's origin.</param>
        /// <returns>the same point expressed in world coordinates.</returns>
        public Vector2 GetWorldPoint(Vector2 localPoint)
        {
            //return Math.Mul(ref m_xf, ref localPoint);
            Vector2 u = new Vector2();
            u.X = m_xf.R.col1.X * localPoint.X + m_xf.R.col2.X * localPoint.Y;
            u.Y = m_xf.R.col1.Y * localPoint.X + m_xf.R.col2.Y * localPoint.Y;
            return m_xf.position + u;
        }

        /// <summary>Get the world coordinates of a vector given the local coordinates.</summary>
        /// <param name="localVector">a vector fixed in the body.</param>
        /// <returns>the same vector expressed in world coordinates.</returns>
        public Vector2 GetWorldVector(Vector2 localVector)
        {
#if !OPTIMIZE
            return Math.Mul(m_xf.R, localVector);
#else
            Vector2 u = new Vector2();
            u.X = m_xf.R.col1.X * localVector.X + m_xf.R.col2.X * localVector.Y;
            u.Y = m_xf.R.col1.Y * localVector.X + m_xf.R.col2.Y * localVector.Y;
            return u;
#endif
        }

        /// <summary>Gets a local point relative to the body's origin given a world point.</summary>
        /// <param name="worldPoint">a point in world coordinates.</param>
        /// <returns>the corresponding local point relative to the body's origin.</returns>
        public Vector2 GetLocalPoint(Vector2 worldPoint)
        {
#if !OPTIMIZE
            return Math.MulT(m_xf, worldPoint);
#else
            Vector2 t; t.X = worldPoint.X - m_xf.position.X; t.Y = worldPoint.Y - m_xf.position.Y;
            Vector2 u = new Vector2();
            u.X = t.X * m_xf.R.col1.X + t.Y * m_xf.R.col1.Y; // Vector2.Dot(ref t, ref m_xf.R.col1, out u.X);
            u.Y = t.X * m_xf.R.col2.X + t.Y * m_xf.R.col2.Y; // Vector2.Dot(ref t, ref m_xf.R.col2, out u.Y);
            return u;
#endif
        }

        /// <summary>Gets a local vector given a world vector.</summary>
        /// <param name="worldVector">a vector in world coordinates.</param>
        /// <returns>the corresponding local vector.</returns>
        public Vector2 GetLocalVector(Vector2 worldVector)
        {
#if !OPTIMIZE
            return Math.MulT(m_xf.R, worldVector);
#else
            Vector2 ret = Vector2.Zero;
            ret.X = worldVector.X * m_xf.R.col1.X + worldVector.Y * m_xf.R.col1.Y; // Vector2.Dot(ref worldVector, ref m_xf.R.col1, out ret.X);
            ret.Y = worldVector.X * m_xf.R.col2.X + worldVector.Y * m_xf.R.col2.Y; // Vector2.Dot(ref worldVector, ref m_xf.R.col2, out ret.Y);
            return ret;
#endif
        }

        /// <summary>Get the world linear velocity of a world point attached to this body.</summary>
        /// <param name="worldPoint">a point in world coordinates.</param>
        /// <returns>the world velocity of a point.</returns>
        public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint)
        {
            return m_linearVelocity + Vector2Extensions.Cross(m_angularVelocity, worldPoint - m_sweep.c);
        }

        /// <summary>Get the world velocity of a local point.</summary>
        /// <param name="localPoint">a point in local coordinates.</param>
        /// <returns>the world velocity of a point.</returns>
        public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint)
        {
            return GetLinearVelocityFromWorldPoint(GetWorldPoint(ref localPoint));
        }

        /// <summary>Get the linear damping of the body.</summary>
        public float LinearDamping
        {
            get { return m_linearDamping; }
            set { m_linearDamping = value; }
        }

        /// <summary>Get the angular damping of the body.</summary>
        public float AngularDamping
        {
            get { return m_angularDamping; }
            set { m_angularDamping = value; }
        }

        /// <summary>
        /// Set the type of this body. This may alter the mass and velocity.
        /// </summary>
        /// <param name="type"></param>
        public BodyType Type
        {
            get { return m_type; }
            set
            {
                if (m_type == value) return;

                m_type = value;

                ResetMassData();

                if (m_type == BodyType.STATIC)
                {
                    m_linearVelocity = Vector2.Zero;
                    m_angularVelocity = 0.0f;
                }

                Awake = true;

                m_force = Vector2.Zero;
                m_torque = 0.0f;

                // Since the body type changed, we need to flag contacts for filtering.
                for (Fixture f = m_fixtureList; f != null; f = f.m_next)
                {
                    f.Refilter();
                }
            }
        }

        /// <summary>Should this body be treated like a bullet for continuous collision detection?</summary>
        public bool Bullet
        {
            get { return (m_flags & BodyFlags.e_bulletFlag) == BodyFlags.e_bulletFlag; }
            set
            {
                if (value)
                    m_flags |= BodyFlags.e_bulletFlag;
                else
                    m_flags &= ~BodyFlags.e_bulletFlag;
            }
        }

        public bool SleepingAllowed
        {
            get { return ((m_flags & BodyFlags.e_autoSleepFlag) == BodyFlags.e_autoSleepFlag); }
            set
            {
                if (value)
                {
                    m_flags |= BodyFlags.e_autoSleepFlag;
                }
                else
                {
                    m_flags &= ~BodyFlags.e_autoSleepFlag;
                    Awake = true;
                }
            }
        }

        public bool Awake
        {
            get { return ((m_flags & BodyFlags.e_awakeFlag) == BodyFlags.e_awakeFlag); }
            set
            {
                if (value)
                {
                    if ((m_flags & BodyFlags.e_awakeFlag) == 0)
                    {
                        m_flags |= BodyFlags.e_awakeFlag;
                        m_sleepTime = 0.0f;
                    }
                }
                else
                {
                    m_flags &= ~BodyFlags.e_awakeFlag;
                    m_sleepTime = 0.0f;
                    m_linearVelocity = Vector2.Zero;
                    m_angularVelocity = 0.0f;
                    m_force = Vector2.Zero;
                    m_torque = 0.0f;
                }
            }
        }

        public bool Active
        {
            get { return ((m_flags & BodyFlags.e_activeFlag) == BodyFlags.e_activeFlag); }
            set
            {
                System.Diagnostics.Debug.Assert(m_world.IsLocked() == false);

                if (value == Active) return;

                if (value)
                {
                    m_flags |= BodyFlags.e_activeFlag;

                    // Create all proxies. 
                    BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
                    for (Fixture f = m_fixtureList; f != null; f = f.m_next)
                    {
                        f.CreateProxies(broadPhase, m_xf);
                    }

                    // Contacts are created the next time step. 
                }
                else
                {
                    m_flags &= ~BodyFlags.e_activeFlag;

                    // Destroy all proxies. 
                    BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
                    for (Fixture f = m_fixtureList; f != null; f = f.m_next)
                    {
                        f.DestroyProxies(broadPhase);
                    }

                    // Destroy the attached contacts. 
                    ContactEdge ce = m_contactList;
                    while (ce != null)
                    {
                        ContactEdge ce0 = ce;
                        ce = ce.next;
                        m_world.m_contactManager.Destroy(ce0.contact);
                    }
                    m_contactList = null;
                }
            }
        }

        /// <summary>Is this body prevented from rotating.</summary>
        public bool FixedRotation
        {
            get { return ((m_flags & BodyFlags.e_fixedRotationFlag) == BodyFlags.e_fixedRotationFlag); }
            set
            {
                if (value == FixedRotation) return;

                if (value)
                {
                    m_flags |= BodyFlags.e_fixedRotationFlag;
                }
                else
                {
                    m_flags &= ~BodyFlags.e_fixedRotationFlag;
                }

                ResetMassData();
            }
        }

        /// <summary>Get the list of all fixtures attached to this body.</summary>
        public Fixture FixtureList
        {
            get { return m_fixtureList; }
        }

        /// <summary>Get the list of all joints attached to this body.</summary>
        public JointEdge JointList
        {
            get { return m_jointList; }
        }

        /// <summary>Get the list of all contacts attached to this body.</summary>
        /// <remarks>this list changes during the time step and you may miss some collisions if you don't use ContactListener.</remarks>
        public ContactEdge ContactList
        {
            get { return m_contactList; }
        }

        /// <summary>Get the next body in the world's body list.</summary>
        public Body Next
        {
            get { return m_next; }
        }

        /// <summary>Get the user data pointer that was provided in the body definition.</summary>
        public object UserData
        {
            get { return m_userData; }
            set { m_userData = value; }
        }

        /// <summary>Get the parent world of this body.</summary>
        public World World
        {
            get { return m_world; }
        }


        
        // m_flags
        [Flags]
	    public enum BodyFlags
	    {
		    e_islandFlag		= 0x0001,
            e_awakeFlag         = 0x0002,
		    e_autoSleepFlag		= 0x0004,
		    e_bulletFlag		= 0x0008,
		    e_fixedRotationFlag	= 0x0010,
            e_activeFlag        = 0x0020,
            e_toiFlag           = 0x0040,
	    };

        internal Body(BodyDef bd, World world)
        {
            System.Diagnostics.Debug.Assert(bd.position.IsValid());
            System.Diagnostics.Debug.Assert(bd.linearVelocity.IsValid());
            System.Diagnostics.Debug.Assert(Math.IsValid(bd.angle));
            System.Diagnostics.Debug.Assert(Math.IsValid(bd.angularVelocity));
            System.Diagnostics.Debug.Assert(Math.IsValid(bd.inertiaScale) && bd.inertiaScale >= 0.0f);
            System.Diagnostics.Debug.Assert(Math.IsValid(bd.angularDamping) && bd.angularDamping >= 0.0f);
            System.Diagnostics.Debug.Assert(Math.IsValid(bd.linearDamping) && bd.linearDamping >= 0.0f); 

	        m_flags = 0;

	        if (bd.bullet)
	        {
		        m_flags |= BodyFlags.e_bulletFlag;
	        }
	        if (bd.fixedRotation)
	        {
		        m_flags |= BodyFlags.e_fixedRotationFlag;
	        }
	        if (bd.allowSleep)
	        {
		        m_flags |= BodyFlags.e_autoSleepFlag;
	        }
	        if (bd.awake)
	        {
		        m_flags |= BodyFlags.e_awakeFlag;
	        }
            if (bd.active)
            {
                m_flags |= BodyFlags.e_activeFlag;
            }

	        m_world = world;

	        m_xf.position = bd.position;
	        m_xf.R.Set(bd.angle);

            m_sweep = new Sweep();
	        m_sweep.localCenter = Vector2.Zero;

            m_sweep.a0 = m_sweep.a = bd.angle;
	        m_sweep.c0 = m_sweep.c = Transform.Mul(m_xf, m_sweep.localCenter);

            //m_jointList = null;
            //m_contactList = null;
            //m_prev = null;
            //m_next = null;

	        m_linearVelocity = bd.linearVelocity;
	        m_angularVelocity = bd.angularVelocity;

	        m_linearDamping = bd.linearDamping;
	        m_angularDamping = bd.angularDamping;

	        m_force = Vector2.Zero;
	        m_torque = 0.0f;

            m_sleepTime = 0.0f;

            m_type = bd.type;

            if (m_type == BodyType.DYNAMIC)
            {
                m_mass = 1.0f;
                m_invMass = 1.0f;
            }
            else
            {
                m_mass = 0.0f;
                m_invMass = 0.0f;
            }

	        m_I = 0.0f;
	        m_invI = 0.0f;

	        m_userData = bd.userData;

	        m_fixtureList = null;
	        m_fixtureCount = 0;
        }

        ~Body()
        {
            // shapes and joints are destroyed in b2World::Destroy
        }

        internal void SynchronizeFixtures()
        {
            Transform xf1 = new Transform();
            xf1.R.Set(ref m_sweep.a0);
#if !OPTIMIZE
            xf1.position = m_sweep.c0 - Math.Mul(xf1.R, m_sweep.localCenter);
#else
            xf1.position.X = m_sweep.c0.X - (xf1.R.col1.X * m_sweep.localCenter.X + xf1.R.col2.X * m_sweep.localCenter.Y);
            xf1.position.Y = m_sweep.c0.Y - (xf1.R.col1.Y * m_sweep.localCenter.X + xf1.R.col2.Y * m_sweep.localCenter.Y);
#endif

            BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
            for (Fixture f = m_fixtureList; f != null; f = f.m_next)
            {
                f.Synchronize(broadPhase, ref xf1, ref m_xf);
            }
        }

        internal void SynchronizeTransform()
        {
#if !OPTIMIZE
            m_xf.R.Set(m_sweep.a);
            m_xf.position = m_sweep.c - Math.Mul(m_xf.R, m_sweep.localCenter);

#else
            float c = (float)System.Math.Cos(m_sweep.a), s = (float)System.Math.Sin(m_sweep.a);
            m_xf.R.col1.X = c; m_xf.R.col2.X = -s;
            m_xf.R.col1.Y = s; m_xf.R.col2.Y = c;

            m_xf.position.X = m_sweep.c.X - (m_xf.R.col1.X * m_sweep.localCenter.X + m_xf.R.col2.X * m_sweep.localCenter.Y);
            m_xf.position.Y = m_sweep.c.Y - (m_xf.R.col1.Y * m_sweep.localCenter.X + m_xf.R.col2.Y * m_sweep.localCenter.Y);
#endif
        }

        internal bool ShouldCollide(Body other)
        {
            // At least one body should be dynamic.
            if (m_type != BodyType.DYNAMIC && other.m_type != BodyType.DYNAMIC)
            {
                return false;
            }
            
            // Does a joint prevent collision?
            for (JointEdge jn = m_jointList; jn != null; jn = jn.next)
            {
                if (jn.other == other)
                {
                    if (jn.joint.m_collideConnected == false)
                    {
                        return false;
                    }
                }
            }
            
            return true; 
        }

        internal void Advance(float alpha)
        {
            // Advance to the new safe time.
            m_sweep.Advance(alpha);
            m_sweep.c = m_sweep.c0;
            m_sweep.a = m_sweep.a0;
            SynchronizeTransform();
        }

	    internal BodyType m_type;
        internal BodyFlags m_flags;

	    internal int m_islandIndex;

	    internal Transform m_xf;		// the body origin transform
	    internal Sweep m_sweep;	// the swept motion for CCD

	    internal Vector2 m_linearVelocity;
	    internal float m_angularVelocity;

	    internal Vector2 m_force;
	    internal float m_torque;

	    public World m_world;
	    public Body m_prev;
	    public Body m_next;

	    internal Fixture m_fixtureList;
	    internal int m_fixtureCount;

	    internal JointEdge m_jointList;
	    internal ContactEdge m_contactList;

	    internal float m_mass, m_invMass;
        // Rotational inertia about the center of mass.
	    internal float m_I, m_invI;

	    internal float m_linearDamping;
	    internal float m_angularDamping;

	    internal float m_sleepTime;

	    object m_userData;
    };
}
